#include "Simple3D.h"
#include "lang/StringFormat.hpp"
#include "dsp/BufferDelayer.h"
#include "dsp/IIRUtil.h"
#include "util/Util.h"
using namespace yzrilyzr_lang;
using namespace yzrilyzr_util;
namespace yzrilyzr_dsp{
	u_sample Simple3D::procDsp(u_sample v){
		if(channel == 0){
			lDelayBuf.ensureCapacity(lDelay);
			lDelayBuf.write(v);
			v=BufferDelayer::linearDelay(lDelayBuf, lDelay);
			v=distanceFilterL.procDsp(v);
			v=lHeadShadowFilter.procDsp(v);
			v=lPinnaFilter.procDsp(v);
			return v;
		} else if(channel == 1){
			rDelayBuf.ensureCapacity(rDelay);
			rDelayBuf.write(v);
			u_sample v=BufferDelayer::linearDelay(rDelayBuf, rDelay);
			v=distanceFilterR.procDsp(v);
			v=rHeadShadowFilter.procDsp(v);
			v=rPinnaFilter.procDsp(v);
			return v;
		}
		return 0;
	}
	void Simple3D::init(u_sample_rate sampleRate){
		this->sampleRate=sampleRate;
		distanceFilterL.init(sampleRate);
		distanceFilterR.init(sampleRate);
		lHeadShadowFilter.init(sampleRate);
		rHeadShadowFilter.init(sampleRate);
		lPinnaFilter.init(sampleRate);
		rPinnaFilter.init(sampleRate);
	}
	void Simple3D::resetMemory(){
		lDelayBuf.fill(0);
		rDelayBuf.fill(0);
		distanceFilterL.resetMemory();
		distanceFilterR.resetMemory();
		lHeadShadowFilter.resetMemory();
		rHeadShadowFilter.resetMemory();
		lPinnaFilter.resetMemory();
		rPinnaFilter.resetMemory();
	}
	std::shared_ptr<DSP> Simple3D::cloneDSP(){
		return std::make_shared<Simple3D>();
	}
	void Simple3D::cloneParam(DSP * obj1){
		if(auto other=dynamic_cast<Simple3D *>(obj1)){
			x=other->x;
			y=other->y;
			z=other->z;
			yaw=other->yaw;
			pitch=other->pitch;
			distance=other->distance;
		}
	}
	void Simple3D::posChange(){
		u_sample speed_sound=343;
		u_sample halfHeadWidth=0.1;
		u_sample tmp=y * y + z * z;
		u_sample xx=(x + halfHeadWidth);
		lDelay=std::sqrt(xx * xx + tmp) / speed_sound * sampleRate;
		xx=(x - halfHeadWidth);
		rDelay=std::sqrt(xx * xx + tmp) / speed_sound * sampleRate;
		u_freq distanceFreq=IIRUtil::limitFreq(sampleRate, 20000.0 / log(distance));
		IIRUtil::biquad(distanceFilterL, distanceFreq, sampleRate, 0.5, LOWPASS, 0);
		IIRUtil::biquad(distanceFilterR, distanceFreq, sampleRate, 0.5, LOWPASS, 0);
		// =================================================================
		// 1. 头影效应 (Head Shadow Effect) 模拟
		// =================================================================
		// 当声音在侧面时，对侧耳朵会收到更少的高频信号。
		// 我们用一个低通滤波器来模拟，其截止频率根据声源的水平位置(yaw)动态变化。

		// a. 计算左右耳的头影滤波强度
		// yaw = -0.5 (左) -> left_strength ~ 0 (影响小), right_strength ~ 1 (影响大)
		// yaw =  0.0 (前) -> left_strength ~ 0.5, right_strength ~ 0.5
		// yaw =  0.5 (右) -> left_strength ~ 1 (影响大), right_strength ~ 0 (影响小)
		u_sample left_shadow_strength=Util::clamp01(std::abs(yaw + 0.5) / 1.0); // 归一化到 [0, 1]
		u_sample right_shadow_strength=Util::clamp01(std::abs(yaw - 0.5) / 1.0);

		// b. 根据强度计算左右耳的低通滤波器截止频率
		// 当强度为0时，截止频率很高(接近Nyquist频率)，几乎不滤波。
		// 当强度为1时，截止频率降低到2kHz，模拟强烈的高频衰减。
		u_freq max_shadow_cutoff=sampleRate * 0.4; // 较高的截止频率
		u_freq min_shadow_cutoff=2000.0;          // 较低的截止频率 (2kHz)
		u_freq l_shadow_cutoff=max_shadow_cutoff - (max_shadow_cutoff - min_shadow_cutoff) * left_shadow_strength;
		u_freq r_shadow_cutoff=max_shadow_cutoff - (max_shadow_cutoff - min_shadow_cutoff) * right_shadow_strength;

		// c. 应用滤波器
		// 使用低通滤波器，并限制截止频率以确保稳定性
		l_shadow_cutoff=IIRUtil::limitFreq(sampleRate, l_shadow_cutoff);
		r_shadow_cutoff=IIRUtil::limitFreq(sampleRate, r_shadow_cutoff);

		IIRUtil::biquad(lHeadShadowFilter, l_shadow_cutoff, sampleRate, 0.707, LOWPASS, 0);
		IIRUtil::biquad(rHeadShadowFilter, r_shadow_cutoff, sampleRate, 0.707, LOWPASS, 0);

		// =================================================================
		// 2. 耳廓效应 (Pinna Effect) 模拟
		// =================================================================
		// 耳廓的形状导致对来自不同垂直角度的声音有特定的频谱"凹陷"。
		// 我们用一个带阻滤波器(Notch Filter)来模拟这个主要的凹陷。

		// a. 根据俯仰角(pitch)调整带阻滤波器的中心频率和Q值
		// pitch = -1.0 (下方) -> 凹陷频率较低
		// pitch =  0.0 (水平) -> 凹陷频率中等
		// pitch =  1.0 (上方) -> 凹陷频率较高
		// 典型的凹陷频率在 5kHz 到 10kHz 之间。
		u_sample base_notch_freq=7500.0; // 基础频率 (7.5kHz)
		u_sample freq_range=3000.0;     // 频率变化范围 (3kHz)

		u_freq notch_freq=base_notch_freq + pitch * freq_range;

		// Q值决定了凹陷的尖锐程度。可以让它也随pitch轻微变化，增加真实感。
		u_sample notch_Q=10.0 - std::abs(pitch) * 5.0; // Q值在2.0到3.0之间变化

		// b. 限制频率范围以确保滤波器稳定性
		notch_freq=IIRUtil::limitFreq(sampleRate, notch_freq);

		// c. 应用滤波器到左右耳
		// 注意：耳廓效应对于左右耳是相似的，主要由垂直角度决定。
		// 因此，左右耳使用相同的带阻滤波器参数。
		IIRUtil::biquad(lPinnaFilter, notch_freq, sampleRate, notch_Q, NOTCH, 0);
		IIRUtil::biquad(rPinnaFilter, notch_freq, sampleRate, notch_Q, NOTCH, 0);
	}
	String Simple3D::toString()const{
		return StringFormat::object2string("Simple3D");
	}
}